package de.adson.navigation;
import javax.swing.tree.DefaultMutableTreeNode;

//import javax.swing.tree.MutableTreeNode;

public class Router {
	Target target = new Target();
	Parameter para = new Parameter();
	double min_dist;
	long zstVorher = 0;
	long zstNachher = 0;
	Payload start = new Payload();
	Polaren polar = new Polaren("Polaren.txt");

	/**
	 * @param args
	 */
	public static void main(String[] args) {
		new Router(0, 0, 0, 20, 20);
	}

	class Parameter {
		short baumTiefe = 4;
		float intervall = 1f;// in Stunden
		short baumBreite = 25;
		short sektor = 100;
		short noGoSektor = 60;
	}

	class Payload {
		float x;
		float y;
		short ws;
		short wd;
		float speed;
		short course;
		short level;
		float remainingDistance;
		float t;
		String name;
	}

	class Target {
		float x;
		float y;
	}

	public Router(float t, float x, float y, float tx, float ty) {
		zstVorher = System.currentTimeMillis();
		target.x = tx;
		target.y = ty;
		start.x = x;
		start.y = y;
		start.t = t;
		start.ws = 0;
		start.wd = 0;
		start.level = 0;
		start.name = "0";
		float foo = (float) Math.pow(para.baumBreite, para.baumTiefe);
		foo = foo + (foo - 1) / (para.baumBreite - 1);
		System.out.println("calculating " + foo + " tree nodes");

		tree();

		zstNachher = System.currentTimeMillis();
		System.out.println("calculation time: "
				+ ((zstNachher - zstVorher) / 1000) + " sec");
	}

	void tree() {
		DefaultMutableTreeNode root = new DefaultMutableTreeNode();

		start.remainingDistance = (float) Math.sqrt(Math.pow(
				target.y - start.y, 2) + Math.pow(target.x - start.x, 2));
		min_dist = start.remainingDistance;

		System.out.println(start.name + " " + start.level + " " + start.course
				+ " " + start.speed + " " + start.x + " " + start.y + " "
				+ start.remainingDistance);

		root.setUserObject(start);
		next_level(root);
	}

	void next_level(DefaultMutableTreeNode parent) {
		Payload parent_payload = new Payload();
		parent_payload = (Payload) parent.getUserObject();
		Payload payload = new Payload();
		Payload payloads[] = new Payload[para.baumBreite];

		payloads = calc_payloads(parent_payload);
		for (short i = 0; i < payloads.length; i++) {
			payload = payloads[i];
			if (payload != null) {
				//System.out.println("not null " + i);
				DefaultMutableTreeNode child = new DefaultMutableTreeNode();
				child.setUserObject(payload);
				parent.insert(child, parent.getChildCount());

				/*System.out.println(payload.name + " " + payload.t + " "
						+ payload.course + " " + payload.speed + " "
						+ payload.x + " " + payload.y + " "
						+ payload.remainingDistance);*/

				if (payload.remainingDistance <= min_dist) {
					min_dist = payload.remainingDistance;
					System.out.println("found: " + payload.name + " "
							+ payload.level + " " + payload.course + " "
							+ payload.speed + " " + payload.x + " " + payload.y
							+ " " + payload.remainingDistance);
				}
				if (payload.level < para.baumTiefe)
					next_level(child);
			}
		}
	}

	Payload[] calc_payloads(Payload pp) {
		short dir2target;
		short dir;
		float speed;
		short twd;
		float tws;

		Payload payloads[] = new Payload[para.baumBreite];

		dir2target = (short) (Math.toDegrees(Math.atan((target.y - pp.y)
				/ (target.x - pp.x))));
		// System.out.println(dir2target);

		dir = dir2target;
		for (short i = 0; i < para.baumBreite; i++) {
			twd = 225;
			tws = 10;
			dir = (short) (dir + para.sektor / (para.baumBreite - 1) * i
					* Math.pow(-1, i));
			// dir = (short) (dir+5);
			if (Math.abs(dir - twd) >= para.noGoSektor / 2) {
				speed = get_speed(pp.level, twd, tws, dir, pp.x, pp.y);
				// System.out.println(dir+" "+speed);
				payloads[i] = new Payload();
				payloads[i].x = (float) (pp.x + para.intervall * speed
						* Math.cos(Math.toRadians(dir)));
				payloads[i].y = (float) (pp.y + para.intervall * speed
						* Math.sin(Math.toRadians(dir)));
				payloads[i].course = dir;
				payloads[i].speed = speed;
				payloads[i].level = (short) (pp.level + 1);
				// payloads[i].name = pp.name +"-"+ i;
				payloads[i].name = pp.name + ":" + dir;
				payloads[i].remainingDistance = (float) Math.sqrt(Math.pow(
						target.y - payloads[i].y, 2)
						+ Math.pow(target.x - payloads[i].x, 2));
				//System.out.println("in  " + i + " " + dir);
			} else {
				//System.out.println("out " + i + " " + dir);
	}
		}
		return payloads;
	}

	float get_speed(float t, short twd, float tws, short dir, float x, float y) {
		float speed;
		short twa = (short) Math.abs(twd - dir);
		speed = (float) polar.getTargetSpeed(tws, twa);
		return (speed);
	}
}
